// Copyright (c) 2012, 2013, 2014 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <cstdlib>

#include "openMVG/sfm/pipelines/global/sfm_global_engine_relative_motions.hpp"
#include "openMVG/system/timer.hpp"
#include "openMVG/cameras/Cameras_Common_command_line_helper.hpp"
#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/cameras/Camera_Pinhole_Radial.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"

using namespace std;
using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::cameras;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

std::string sDataRootDir { "D:/south-building/" };
std::string sImageList { sDataRootDir + "/localizer/images_list.txt" };
std::string sBundlerOutFile { sDataRootDir + "/localizer/model_bundler.out" };

std::string sSparseCameras { "/sparse/cameras.txt" };
std::string sSparseImages { "/sparse/images.txt" };
std::string sSparsePoints { "/sparse/points3D.txt" };

// merge intrinsic ids
std::string sfm_json_converted { sDataRootDir + "/model/sfm_data_converted.json" };
std::string sfm_json_init { sDataRootDir + "/matches/sfm_data.json" };
std::string sfm_json { sDataRootDir + "/reconstruction/sfm_data.json" };

std::string sfm_json_robust { sDataRootDir + "/reconstruction/robust.json" };
std::string sfm_json_robust_colorized { sDataRootDir + "/reconstruction/robust_colorized.ply" };

struct Image
{
  int image_id;
  int camera_id;
  Mat3 r;
  Vec3 t;
  std::string img_name;
};

struct Intrinsic
{
  double w, h, f, px, py;
  double d[3];
  Intrinsic(double _w, double _h, double _f, double _px, double _py, double _d0, double _d1, double _d2) :w(_w), h(_h), f(_f), px(_px), py(_py) {
    d[0] = _d0; d[1] = _d1; d[2] = _d2;
  }
  Intrinsic() {
    w = h = f = px = py = d[0] = d[1] = d[2] = 0.0;
  }
  ~Intrinsic() {}
};


bool ConvertToBundler(const string& sfm_json, const string& list, const string& bundler_out) {
  SfM_Data sfm_data;;
  if (!Load(sfm_data, sfm_json, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "The input SfM_Data file \"" << sfm_json << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "views size: " << sfm_data.views.size() << std::endl;
  std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
  std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;
  std::cout << "structure size: " << sfm_data.structure.size() << std::endl;

  int num_cameras = sfm_data.views.size();
  int num_points = sfm_data.structure.size();
  std::vector<bool> image_db(num_cameras, true);

  for (int i = 7; i < num_cameras; ++i) {
    if (i % 7 == 0) {
      image_db[i] = false;
    }
  }

  std::ofstream ofs1 { list, ios::binary }, ofs2 { bundler_out, ios::binary };
  if (!ofs1.is_open() || !ofs2.is_open()) {
    std::cerr << std::endl
      << "image list file or bundler out file open fail:\n"
      << list << std::endl << bundler_out << std::endl;
    return EXIT_FAILURE;
  }

  ofs2 << std::scientific;
  ofs2 << "# Bundle file v0.3\n"
    << num_cameras << " " << num_points << std::endl;

  int cnt = 0;
  std::map<int, int> view_key_id;
  for (const auto& view : sfm_data.views) {
    ofs1 << stlplus::filename_part(view.second->s_Img_path) << " "
      << image_db[cnt] << " " << view.second->ui_width << " " << view.second->ui_height << std::endl;

    const auto& intr = sfm_data.intrinsics.at(view.second->id_intrinsic);
    double d[3]{0.0, 0.0, 0.0};
    intr->distortion(d);
    ofs2 << intr->focal() << " " << d[0] << " " << d[1] << std::endl;

    const auto& pose = sfm_data.poses.at(view.second->id_pose);
    Mat3 D { Mat3::Identity()};
    D(2, 2) = -1.0;
    Vec3 t = -pose.rotation_ * pose.center_;
    Mat3 rotation = D * pose.rotation_;
    Vec3 translation = D * t;

    ofs2 << rotation(0, 0) << " " << rotation(0, 1) << " " << rotation(0, 2) << "\n"
      << rotation(1, 0) << " " << rotation(1, 1) << " " << rotation(1, 2) << "\n"
      << rotation(2, 0) << " " << rotation(2, 1) << " " << rotation(2, 2) << "\n"
      << translation[0] << " " << translation[1] << " " << translation[2] << std::endl;

    view_key_id[view.first] = cnt;
    ++cnt;
  }
  ofs1.close();
  std::cout << "write image names number: " << cnt << std::endl;
  if (cnt != num_cameras) {
    std::cerr << "images number error: " << num_cameras << std::endl;
    return EXIT_FAILURE;
  }

  // load point color
  std::vector<int> colors;
  colors.reserve(3 * num_points);
  std::ifstream ifs { sfm_json_robust_colorized };
  if (!ifs.is_open()) {
    std::cerr << "open file fail: " + sfm_json_robust_colorized << std::endl;
    std::cerr << "write rgb value to zero: " << std::endl;
    colors.resize(3 * num_points, 0);
  } else {
    std::string str;
    getline(ifs, str);  // ply
    getline(ifs, str);  // format ascii 1.0
    getline(ifs, str);  // element vertex 454069
    getline(ifs, str);  // property double x
    getline(ifs, str);  // property double y
    getline(ifs, str);  // property double z
    getline(ifs, str);  // property uchar red
    getline(ifs, str);  // property uchar green
    getline(ifs, str);  // property uchar blue
    getline(ifs, str);  // end_header

    double d;
    int i;
    while (ifs >> d) {
      ifs >> d >> d;
      ifs >> i; colors.push_back(i);
      ifs >> i; colors.push_back(i);
      ifs >> i; colors.push_back(i);
    }
    ifs.close();
  }

  // start to write points
  int k = 0;
  for (const auto& s : sfm_data.structure) {
    ofs2 << std::scientific;
    ofs2 << s.second.X[0] << " "
      << s.second.X[1] << " "
      << s.second.X[2] << "\n";
    ofs2 << std::fixed;
    ofs2 << colors[3 * k] << " "
      << colors[3 * k + 1] << " "
      << colors[3 * k + 2] << "\n";
    ofs2 << s.second.obs.size() << " ";
    for (const auto& observe : s.second.obs) {
      ofs2 << view_key_id[observe.first] << " "
        << observe.second.id_feat << " "
        << observe.second.x[0] << " "
        << observe.second.x[1] << " ";
    }
    ofs2 << std::endl;
    ++k;
  }
  ofs2.close();
  std::cout << "write points number: " << k << std::endl;
  if (k != num_points) {
    std::cerr << "write points number error: " << num_points << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

/*
// use only bundler output data
bool ConvertFromBundler(){
// Load input SfM_Data scene
SfM_Data sfm_data;
sfm_data.s_root_path = sDataRootDir;

std::vector<std::string> img_names;
img_names.reserve(8000);
std::vector<int> img_height_width;
img_height_width.reserve(16000);

std::ifstream ifs(sDataRootDir + sImgNameList, std::ios::in);
if (!ifs.is_open()) {
std::cout << "open file fail: " << sDataRootDir + sImgNameList << std::endl;
return EXIT_FAILURE;
}

std::string img_name_tmp;
int h, w;
while (ifs >> img_name_tmp) {
ifs >> h >> w;
img_height_width.push_back(h);
img_height_width.push_back(w);
img_names.push_back("images/" + img_name_tmp);
}
std::cout << "load image name: " << img_names.size() << std::endl;
std::cout << "load image size: " << img_height_width.size() << std::endl;
ifs.close();



// read bundler data
int num_camera, num_point;
std::ifstream instream(sDataRootDir + sBundlerOutFile, std::ios::in);
if (!instream.is_open()) {
std::cout << "open bundler fail: " << sDataRootDir + sBundlerOutFile << std::endl;
return EXIT_FAILURE;
}

std::string line_in_file;
getline(instream, line_in_file);//header
instream >> num_camera >> num_point;

if (img_names.size() != num_camera) {
std::cout << "num_camera error: " << num_camera << std::endl;
return EXIT_FAILURE;
}

// read camera parameter
for (int i = 0; i < num_camera; ++i) {
int h = img_height_width[2 * i];
int w = img_height_width[2 * i + 1];
std::shared_ptr<View> ptr_view = std::make_shared<View>();
ptr_view->id_intrinsic = image_intrinsic[i];
ptr_view->id_pose = i;
ptr_view->id_view = i;
ptr_view->s_Img_path = img_names[i];
ptr_view->ui_height = h;
ptr_view->ui_width = w;
sfm_data.views[i] = ptr_view;

geometry::Pose3 pose;

double f, k1, k2;
instream >> f >> k1 >> k2;

instream >> pose.rotation_(0, 0)
>> pose.rotation_(0, 1)
>> pose.rotation_(0, 2)
>> pose.rotation_(1, 0)
>> pose.rotation_(1, 1)
>> pose.rotation_(1, 2)
>> pose.rotation_(2, 0)
>> pose.rotation_(2, 1)
>> pose.rotation_(2, 2);

Vec3 translation;
instream >> translation[0] >> translation[1] >> translation[2];
pose.center_ = -(pose.rotation().transpose() * translation);

Mat3 D(Mat3::Identity());
D(2, 2) = -1.0;
pose.rotation_ = D*pose.rotation_;

sfm_data.poses[i] = pose;

//std::shared_ptr<cameras::IntrinsicBase> ptr_intrinsic = std::make_shared<Pinhole_Intrinsic_Radial_K3>
//  ( w, h, f, w / 2.0, h / 2.0, 0.0, 0.0, 0.0 );  // setup no distortion as initial guess
//sfm_data.intrinsics[i] = ptr_intrinsic;
}
instream.close();

std::cout << "views size: " << sfm_data.views.size() << std::endl;
std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;


// save to json format
Save(sfm_data, stlplus::create_filespec(sOutDir, "sfm_data_converted", ".json"), ESfM_Data(ALL));
//return 0;


return EXIT_SUCCESS;
}
*/


int main(int argc, char **argv) {
  std::cout << "input argc: " << argc << std::endl;
  for (int i = 0; i < argc; ++i) {
    std::cout << argv[i] << std::endl;
  }

  if (argc > 1) {
    sDataRootDir = std::string { argv[1] };
  }
  if (argc > 3) {
    sfm_json_robust = std::string { argv[2] };
    sfm_json_robust_colorized = std::string { argv[3] };
  }
  std::cout << "\nYou called\n" << string(argv[0]) << std::endl;
  std::cout << sfm_json_robust << "\n" << sImageList << "\n" << sBundlerOutFile << std::endl;

  ConvertToBundler(sfm_json_robust, sImageList, sBundlerOutFile);
  
  return EXIT_SUCCESS;
}

